#include "matrix4.h"
#include <iomanip>
using std::ostream;

namespace fbrender {

    Matrix4 Matrix4::ZERO(0, 0, 0, 0,
                                0, 0, 0, 0,
                                0, 0, 0, 0,
                                0, 0, 0, 0);

    Matrix4 Matrix4::IDENTITY(1, 0, 0, 0,
                                0, 1, 0, 0,
                                0, 0, 1, 0,
                                0, 0, 0, 1);

    Matrix4 Matrix4::transpose() const
    {
        Matrix4 result;
        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {
                result.m[j][i] = m[i][j];
            }
        }

        return result;
    }

    Matrix4 Matrix4::inverse() const
    {    
        Matrix4 result;
        Real det;
        int i;

        result._m[0] = _m[5]  * _m[10] * _m[15] - 
                 _m[5]  * _m[11] * _m[14] - 
                 _m[9]  * _m[6]  * _m[15] + 
                 _m[9]  * _m[7]  * _m[14] +
                 _m[13] * _m[6]  * _m[11] - 
                 _m[13] * _m[7]  * _m[10];

        result._m[4] = -_m[4]  * _m[10] * _m[15] + 
                  _m[4]  * _m[11] * _m[14] + 
                  _m[8]  * _m[6]  * _m[15] - 
                  _m[8]  * _m[7]  * _m[14] - 
                  _m[12] * _m[6]  * _m[11] + 
                  _m[12] * _m[7]  * _m[10];

        result._m[8] = _m[4]  * _m[9] * _m[15] - 
                 _m[4]  * _m[11] * _m[13] - 
                 _m[8]  * _m[5] * _m[15] + 
                 _m[8]  * _m[7] * _m[13] + 
                 _m[12] * _m[5] * _m[11] - 
                 _m[12] * _m[7] * _m[9];

        result._m[12] = -_m[4]  * _m[9] * _m[14] + 
                   _m[4]  * _m[10] * _m[13] +
                   _m[8]  * _m[5] * _m[14] - 
                   _m[8]  * _m[6] * _m[13] - 
                   _m[12] * _m[5] * _m[10] + 
                   _m[12] * _m[6] * _m[9];

        result._m[1] = -_m[1]  * _m[10] * _m[15] + 
                  _m[1]  * _m[11] * _m[14] + 
                  _m[9]  * _m[2] * _m[15] - 
                  _m[9]  * _m[3] * _m[14] - 
                  _m[13] * _m[2] * _m[11] + 
                  _m[13] * _m[3] * _m[10];

        result._m[5] = _m[0]  * _m[10] * _m[15] - 
                 _m[0]  * _m[11] * _m[14] - 
                 _m[8]  * _m[2] * _m[15] + 
                 _m[8]  * _m[3] * _m[14] + 
                 _m[12] * _m[2] * _m[11] - 
                 _m[12] * _m[3] * _m[10];

        result._m[9] = -_m[0]  * _m[9] * _m[15] + 
                  _m[0]  * _m[11] * _m[13] + 
                  _m[8]  * _m[1] * _m[15] - 
                  _m[8]  * _m[3] * _m[13] - 
                  _m[12] * _m[1] * _m[11] + 
                  _m[12] * _m[3] * _m[9];

        result._m[13] = _m[0]  * _m[9] * _m[14] - 
                  _m[0]  * _m[10] * _m[13] - 
                  _m[8]  * _m[1] * _m[14] + 
                  _m[8]  * _m[2] * _m[13] + 
                  _m[12] * _m[1] * _m[10] - 
                  _m[12] * _m[2] * _m[9];

        result._m[2] = _m[1]  * _m[6] * _m[15] - 
                 _m[1]  * _m[7] * _m[14] - 
                 _m[5]  * _m[2] * _m[15] + 
                 _m[5]  * _m[3] * _m[14] + 
                 _m[13] * _m[2] * _m[7] - 
                 _m[13] * _m[3] * _m[6];

        result._m[6] = -_m[0]  * _m[6] * _m[15] + 
                  _m[0]  * _m[7] * _m[14] + 
                  _m[4]  * _m[2] * _m[15] - 
                  _m[4]  * _m[3] * _m[14] - 
                  _m[12] * _m[2] * _m[7] + 
                  _m[12] * _m[3] * _m[6];

        result._m[10] = _m[0]  * _m[5] * _m[15] - 
                  _m[0]  * _m[7] * _m[13] - 
                  _m[4]  * _m[1] * _m[15] + 
                  _m[4]  * _m[3] * _m[13] + 
                  _m[12] * _m[1] * _m[7] - 
                  _m[12] * _m[3] * _m[5];

        result._m[14] = -_m[0]  * _m[5] * _m[14] + 
                   _m[0]  * _m[6] * _m[13] + 
                   _m[4]  * _m[1] * _m[14] - 
                   _m[4]  * _m[2] * _m[13] - 
                   _m[12] * _m[1] * _m[6] + 
                   _m[12] * _m[2] * _m[5];

        result._m[3] = -_m[1] * _m[6] * _m[11] + 
                  _m[1] * _m[7] * _m[10] + 
                  _m[5] * _m[2] * _m[11] - 
                  _m[5] * _m[3] * _m[10] - 
                  _m[9] * _m[2] * _m[7] + 
                  _m[9] * _m[3] * _m[6];

        result._m[7] = _m[0] * _m[6] * _m[11] - 
                 _m[0] * _m[7] * _m[10] - 
                 _m[4] * _m[2] * _m[11] + 
                 _m[4] * _m[3] * _m[10] + 
                 _m[8] * _m[2] * _m[7] - 
                 _m[8] * _m[3] * _m[6];

        result._m[11] = -_m[0] * _m[5] * _m[11] + 
                   _m[0] * _m[7] * _m[9] + 
                   _m[4] * _m[1] * _m[11] - 
                   _m[4] * _m[3] * _m[9] - 
                   _m[8] * _m[1] * _m[7] + 
                   _m[8] * _m[3] * _m[5];

        result._m[15] = _m[0] * _m[5] * _m[10] - 
                  _m[0] * _m[6] * _m[9] - 
                  _m[4] * _m[1] * _m[10] + 
                  _m[4] * _m[2] * _m[9] + 
                  _m[8] * _m[1] * _m[6] - 
                  _m[8] * _m[2] * _m[5];

        det = _m[0] * result._m[0] + _m[1] * result._m[4] + _m[2] * result._m[8] + _m[3] * result._m[12];

        if (det == 0) return result;

        det = 1.0 / det;

        for (i = 0; i < 16; i++)
            result._m[i] = result._m[i] * det;

        return result;
    }

    ostream& operator<<(ostream& os, const Matrix4& mat)
    {
        os << "{";
        for (int i = 0; i < 4; i++) {
            if (i) os << " ";
            os << "{";
            for (int j = 0; j < 4; j++) { 
                if (j) os << ", ";
                os << std::setw(6) << mat[i][j];
            }
            os << "}";
            if (i != 3) os << std::endl;
        }
        os << "}";
        return os;
    }

}

